#ifndef RSIM_COORDINATEPROJECTION_H
#define RSIM_COORDINATEPROJECTION_H
/* -------------------------------------------------------------------------- *
 * File: CoordinateProjection.h                                               *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "LinAlgDecl.h"
#include "List.h"
#include "CoordinateProjectionUtils.h"
#include "Vector.h"

#include <iostream>


namespace RSIM{

template <class T> class List;
class Constraint;
class MultibodySystem;
class State;
        
class CoordinateProjection:public CoordinateProjectionUtils{
        private:
                List <Constraint*>& constraints_;
                const List <List<int> >& ccouple_;
                const List <MultibodySystem*>& system_;
                List <State*>& state_;
                
                /** Constraint Jacobian*/
                Matrix *CJac_;
                
                /** Position level errors stored in Cq_*/
                Vector *Cq_;
                
                // CJac_*delq_ = -Cq_
                Vector *delq_;
        
                List <List <int> > sid_;
                
                Vector_<int> ConstraintToUMap_;
                
                Vector PositionErrorNorms_;                
                Vector VelocityErrorNorms_;                                
                                
                // Debug function
                void printDebugInformation()const;
               
                // This calculates constraint jacobian for all the constraints
                void calcConstraintJacobians_Tobedeleted();
                
                // This calculates a single constraint jacobian for the set of
                // coupled constraints denoted by index.
                void calcOneConstraintJacobian(const int& index);                                
                
                int Nccouple_;
                
                // Illegal Constructor
                CoordinateProjection();
                
                // This is the acceptable value of constraint tolerance.
                double CTol_;
                
                /** This is the maximum number of iterations that are attempted to meet the constraint
                tolerance during coordinate projection. Default value is 50.*/
                int MaxCprIter_;
                
                // This function is identical to that of SystemData::updsystemPosition()
                void updSystemPosition();
                
        public:
                /** This function computes the dimensions of constraint jacobians. Constraints which
                are decoupled (which do not have any joint in common) are stored in different Jacobians.
                This function also figures out the indices of constraint u's in the jacobian*/
                void init();
                
                CoordinateProjection(const List<List<int> >& ccouple,
                                     const List<MultibodySystem*>& system,
                                     List<Constraint*>& constraints,
                                     List <State*>& state):
                                     constraints_(constraints), ccouple_(ccouple),
                                     system_(system),state_(state),
                                     CJac_(NULL),Cq_(NULL),delq_(NULL),
                                     CTol_(1.0E-6),MaxCprIter_(2000){}
                
                void calcPositionLevelErrors();
                
                void attemptSatisfyPositionLevelConstraint();
                
                void setConstraintTolerance(const double& Tol);
                
                /** This function can be used to change the number of iterations attempted to satisfy
                the constraints upto the desired tolerance before giving up. Default value is 50 */
                void setMaxIterations(const int& niter);
                
                ~CoordinateProjection();
	
}; // class CoordinateProjection
}// namespace RSIM

#endif
